/*************************************************
 *  BMA180 Library
 *************************************************/

#include "lib_bma180.h"

void BMA180_init() {
    unsigned short temp;

    // Set ee_w bit on ctrl_reg0 register (Enable EEPROM writing)
    I2C1_Start();
    I2C1_Wr(BMA180_WRITE_ADDR);      // Write mode
    I2C1_Wr(0x0D);
    I2C1_Stop();
    I2C1_Start();
    I2C1_Wr(BMA180_READ_ADDR);       // Read Mode
    temp = I2C1_Rd(0);
    I2C1_Stop();
    I2C1_Start();
    I2C1_Wr(BMA180_WRITE_ADDR);      // Write mode
    I2C1_Wr(0x0D);
    I2C1_Wr(temp | 0x10);
    I2C1_Stop();
    Delay_ms(50);

    // Set mode_config on ee_tco_z register (Set mode to Ultra low noise)
    I2C1_Start();
    I2C1_Wr(BMA180_WRITE_ADDR);      // Write mode
    I2C1_Wr(0x50);
    I2C1_Stop();
    I2C1_Start();
    I2C1_Wr(BMA180_READ_ADDR);       // Read Mode
    temp = I2C1_Rd(0);
    I2C1_Stop();
    I2C1_Start();
    I2C1_Wr(BMA180_WRITE_ADDR);      // Write mode
    I2C1_Wr(0x50);
    I2C1_Wr((temp & 0xF8) | 0x01);
    I2C1_Stop();
    Delay_ms(10);

    // Set value on bw_tcs register (Set bandwidth to 10Hz)
    I2C1_Start();
    I2C1_Wr(BMA180_WRITE_ADDR);      // Write mode
    I2C1_Wr(0x20);
    I2C1_Stop();
    I2C1_Start();
    I2C1_Wr(BMA180_READ_ADDR);       // Read Mode
    temp = I2C1_Rd(0);
    I2C1_Stop();
    I2C1_Start();
    I2C1_Wr(BMA180_WRITE_ADDR);      // Write mode
    I2C1_Wr(0x20);
    I2C1_Wr(temp & 0x0F);
    I2C1_Stop();
    Delay_ms(10);

    // Set range on offset_lsb1 register (Set range to +/- 2g)
    I2C1_Start();
    I2C1_Wr(BMA180_WRITE_ADDR);      // Write mode
    I2C1_Wr(0x35);
    I2C1_Stop();
    I2C1_Start();
    I2C1_Wr(BMA180_READ_ADDR);       // Read Mode
    temp = I2C1_Rd(0);
    I2C1_Stop();
    I2C1_Start();
    I2C1_Wr(BMA180_WRITE_ADDR);      // Write mode
    I2C1_Wr(0x35);
    I2C1_Wr(((temp & 0xF0) | 0x04));
    I2C1_Stop();
    Delay_ms(10);
}

void BMA180_readAcc(float *acc) {
    unsigned short cnt;
    unsigned short acc_read[6];
    int acc_int[3];

    I2C1_Start();
    I2C1_Wr(BMA180_WRITE_ADDR);      // Write mode
    I2C1_Wr(0x02);
    I2C1_Stop();
    I2C1_Start();
    I2C1_Wr(BMA180_READ_ADDR);       // Read Mode
    for (cnt=0; cnt<6; cnt++) {
        char ack;
        ack = (cnt<5);
        acc_read[cnt] = I2C1_Rd(ack);
    }
    I2C1_Stop();
    
    acc_int[0] = acc_read[1];
    acc_int[0] = acc_int[0] << 8;
    acc_int[0] |= acc_read[0];
    acc_int[0] = acc_int[0] >> 2;
    acc_int[1] = acc_read[3];
    acc_int[1] = acc_int[1] << 8;
    acc_int[1] |= acc_read[2];
    acc_int[1] = acc_int[1] >> 2;
    acc_int[2] = acc_read[5];
    acc_int[2] = acc_int[2] << 8;
    acc_int[2] |= acc_read[4];
    acc_int[2] = acc_int[2] >> 2;
    
    acc[0] = (float)(acc_int[0] / 4096.0);
    acc[1] = (float)(acc_int[1] / 4096.0);
    acc[2] = (float)(acc_int[2] / 4096.0);
}